#include "Controller.h"
#include "DSP2833x_Device.h"
#include "FlashDriver.h"
#include "DataFile.h"
#include "UARTDriverA.h"
#include "UARTDriverB.h"
#include <cstring>
#include <cstdio>

#define TICKSPERSEC 10

extern SDataFile df;
extern FlashDriver flashDriver;
extern UARTDriverA uartDriverA;
extern UARTDriverB uartDriverB;

void Controller::Init()
{
	m_OPState = OFFLINE;
	
	ResetErrors();	
}

void Controller::ResetErrors()
{
	for(int i=0; i!=FS_LAST; i++ )
	{
		m_FailStates[i] = false; // reset errors
	}	
}
 
void Controller::Update()
{
	switch( m_OPState )
	{
		case OFFLINE:
			break;
			
		case GOONLINE:
			// start erase procedure
			flashDriver.BulkErase();
			m_OPState = ERASINGFLASH;
			break;
	
		case ERASINGFLASH:
			if( flashDriver.IsReady() == true )
			{
				// erasing done
				m_OPState = ONLINE;
				m_CurrentReadWritePage = 0; // reset page counter
			}
			break;
			
		case ONLINE:
		{
			// store df to flash
			if( m_CurrentReadWritePage >= 8192 )
			{
				m_OPState = OFFLINE; // memory exhausted
				break;
			}
			if( flashDriver.IsReady() != true ) break; // not ready, error
			Uint32 addressToWrite = m_CurrentReadWritePage*256; // page to write
			unsigned char buffer[128];
			std::memset(buffer, 0, 128); // erase page
			std::memcpy(buffer, &df, sizeof(SDataFile));
			flashDriver.WritePage16(addressToWrite, buffer);
			m_CurrentReadWritePage++;
			break;
		}
		
		case DATATRANSFER:
		{
			if( m_CurrentReadWritePage >= 8192 )
			{
				m_OPState = OFFLINE; // transfer complete
				break;	
			}
			Uint32 addressToRead = m_CurrentReadWritePage*256; // page to read
			unsigned char buffer[128];
			flashDriver.ReadPage16(addressToRead, buffer);
			SDataFile tdf;
			std::memcpy(&tdf, buffer, sizeof(SDataFile));
			// dump to serial port
			std::sprintf((char*)buffer, "$FLASH,%ld,%s,%0.6Lf,%0.6Lf,%0.2f,%d,%0.1f,%0.1f,%0.3f,%0.3f,%0.3f,%0.3f,%0.3f,%0.3f,%0.3f,%0.3f,%0.3f#\r\n", 
				m_CurrentReadWritePage, tdf.SatTime, tdf.Longitude, tdf.Latitude, tdf.Altitude, tdf.SatNum, 
				tdf.Pressure1*1000, tdf.Pressure2*1000, tdf.TemperatureADC1, tdf.TemperatureADC2, tdf.TemperatureExternal,
				df.Levels[0], df.Levels[1], df.Levels[2],df.Rots[0], df.Rots[1], df.Rots[2] );
			uartDriverA.SendData((char*)buffer); // DEBUG
			uartDriverB.SendData((char*)buffer); // DEBUG
			m_CurrentReadWritePage++;
			break;
		}	
		default:
			// do nothing
			break;
	}
}

void Controller::SetState(EStates stateToGo)
{
	switch(stateToGo)
	{
		case ONLINE:
			if( m_OPState == OFFLINE )
			{
				m_StateTimerTick = 1*TICKSPERSEC; // wait 1 secs for online
				m_OPState = GOONLINE;
			}
			break;
		
		case DATATRANSFER:
			if( m_OPState == OFFLINE )
			{
				m_CurrentReadWritePage = 0; // start reading from begining
				m_OPState = DATATRANSFER;
			}
			break;
			
		default:
			// unallowed transistion
			break;
	}
}

Controller::EStates Controller::GetState()
{
	return m_OPState;
}
  
unsigned int Controller::GetFailStateWord()
{
	unsigned int failWord = 0;
	for(int i=FS_LAST-1; i>=0; i-- )
	{
		failWord = failWord << 1; // shift left
		if( m_FailStates[i] )
		{
			failWord = failWord+1;	
		}				
	}
	return failWord;		
}
